/*****************
 * ------------------
 * |CMD|spc|PARAM|\n|
 * ------------------
 *****************/
 
#include <PString.h>

//#include <avr/io.h>
//#include <avr/wdt.h>

//#define WATCHDOG_TIMEOUT WDTO_2S

#define MAX_CMD_LEN 128
char bufInput[MAX_CMD_LEN];
char bufOutput[MAX_CMD_LEN];

// define some offets into the receive buffer for easier parsing
PString strRaw(bufInput,MAX_CMD_LEN);
PString strCmd(bufInput,4);
PString strOpt(bufInput+4,MAX_CMD_LEN-4);
PString strOpt0(bufInput+4,2);
PString strOpt1(bufInput+6,MAX_CMD_LEN-6);
//PString strOpt2(bufInput+8,MAX_CMD_LEN-8);

#define PIN_OUT_BUZZER 7
#define PIN_OUT_OPEN 9
#define PIN_OUT_CLOSE 10
#define PIN_OUT_LED 11
#define PIN_IN_DOOR_CLOSED 8

// interrupt handled pin, must be on pin 2 or 3
#define PIN_IN_CLOSE 2
#define PIN_IN_CLOSE_HYST_MS 2000
volatile boolean pinInClosePressed = false;

#define LOCKDOWN_TIMEOUT_MS (1000*10)
volatile boolean gIsInLockdownMode = false;

#define OPEN_CLOSE_HYSTERESIS (1000*10)
volatile unsigned long gLastCloseTimeMillis=0;

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// parse a received serial command packet
boolean parseCommand() {

  // terminte the command string from the rest
  bufInput[3]=0;
  boolean isOk = true;
  
  if(gIsInLockdownMode) {
    Serial.println("NCK command ignored because of lockdown");
    isOk = false;
    goto abort_parse;
  }

  if(strCmd == "SDC") {
    // set door lock
    bufInput[5]=0; // terminate Opt1
    if(strOpt0 == "0") {
      // open door
      if((isOk = unlockDoor())) {
        Serial.println("ACK SDC 0");
      } else {
        Serial.println("NCK SDC flood");
      }
    } 
    else if(strOpt0 == "1") {
      if(isDoorClosed()) {
        // close door
        if((isOk = lockDoor())) {
          Serial.println("ACK SDC 1");
        } else {
          Serial.println("NCK SDC flood");
        }
      } else {
        isOk = false;
        Serial.println("NCK SDC door is open, close it first");
      }
    } 
    else {
      // invalid
      isOk = false;
      Serial.println("NCK SDC invalid parameter");
    }
  } 
  else if(strCmd == "GDC") {
    // get dor lock status
    // no options
    isOk = false;
    Serial.println("NCK GDC not implemented");
  } 
  else if(strCmd == "GDS") {
    // get door status
    // no option
    if(isDoorClosed()) {
      Serial.println("ACK GDS 1");
    } else {
      Serial.println("ACK GDS 0");
    }
  } 
  else if(strCmd == "STX") {
    // set text on display
    // strOpt == text
    isOk = false;
    Serial.println("NCK STX not implemented");
  } 
  else if(strCmd == "RAW") {
    // raw command
    bufInput[5]=0; // terminate Opt1
    if(strOpt0 == "0") {
      // Open
      if((isOk = unlockDoor())) {
        Serial.println("ACK RAW 0");
      } else {
        Serial.println("NCK RAW flood");
      }
    } 
    else if(strOpt0 == "1") {
      // Close
      if((isOk = lockDoor())) {
        Serial.println("ACK RAW 1");
      } else {
        Serial.println("NCK RAW flood");
      }
    } 
    else if(strOpt0 == "2") {
      // Setup
      isOk = false;
      Serial.println("NCK RAW 2 not implemented");
    } 
    else {
      // invalid
      isOk = false;
      Serial.println("NCK RAW invalid parameter");
    }
  } 
  else {
    // invalid command
    isOk = false;
  }

abort_parse:
  for(int i=0; i<MAX_CMD_LEN; ++i) {
    bufInput[i]=0;
  }
  strRaw.begin();
  return isOk;
}

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// check the reed contact to see if the door
// is in the closed position
boolean isDoorClosed()
{
  if(digitalRead(PIN_IN_DOOR_CLOSED) == LOW) {
    return true;
  }
  return false;
}

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// insturct the keymatic to unlock the door
boolean unlockDoor() {
  unsigned long time = millis();
  if(gLastCloseTimeMillis + OPEN_CLOSE_HYSTERESIS < time) {
    digitalWrite(PIN_OUT_OPEN,HIGH);
    delay(1000);
    digitalWrite(PIN_OUT_OPEN,LOW);
    gLastCloseTimeMillis = time;
    return true;
  }
  return false;
}

/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// instruct the keymatic to lock the door 
boolean lockDoor() {
  unsigned long time = millis();
  if(gLastCloseTimeMillis + OPEN_CLOSE_HYSTERESIS < time) {
    if(isDoorClosed()) {
      digitalWrite(PIN_OUT_CLOSE,HIGH);
      delay(1000);
      digitalWrite(PIN_OUT_CLOSE,LOW);
      gLastCloseTimeMillis = time;
      return true;
    } else {
      return false;
    }
  }
  return false;
}


/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
// perform the time delayed lockdown procedure
// which does not require any wireless devices
// 1) open door
// 2) press lockdown button
//    alarm sounds, display shows warning, lights blink
// 3) leave room
// 4) close door
//    keymatic closes door
boolean commandDoorLockdown() {
  gIsInLockdownMode = true;
  boolean isOk = false;
  
  unsigned long time = millis();
  
  if(gLastCloseTimeMillis + OPEN_CLOSE_HYSTERESIS >= time) {
    // we just received a open or close not too long ago, ignore the request
    ledBlink(25,10);
    isOk = false;
    goto abort_lockdown;
  }

  if(isDoorClosed()) {
    // door is closed
    // to initiate closing sequence
    // the door must be opened
    ledBlink(25,10);
    isOk = false;
    goto abort_lockdown;
  }
  else {
    unsigned long waitTimeStart = millis();
    boolean doorStatusClosed = false;
    // wait till the door gets closed
    while(!(doorStatusClosed = isDoorClosed())) {
      // sound warning countdown
      ledBlink(500,1);
      //TODO: door close hysteresis
      
      if(waitTimeStart+LOCKDOWN_TIMEOUT_MS < millis()) {
        ledBlink(25,10);
        ledBlink(12,20);
        ledBlink(6,40);
        ledBlink(3,80);
        ledBlink(1,160);
        break;
      }
    }
    
    if(doorStatusClosed) {
      ledBlink(100,10);
      // should not happen but potentially we could be open/close hysteresis when calling this.
      // chances are veeeeery small though
      isOk = lockDoor();      
    }
  }
  
  abort_lockdown:
  gIsInLockdownMode = false;
  return isOk;
}


/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
void ledBlink(int delayMs, int repeat) {
  for(int i=0; i<repeat; ++i) {
    digitalWrite(PIN_OUT_LED,HIGH);
    digitalWrite(PIN_OUT_BUZZER,HIGH);
    delay(delayMs);
    digitalWrite(PIN_OUT_LED,LOW);
    digitalWrite(PIN_OUT_BUZZER,LOW);
    delay(delayMs);
  }
}





/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
volatile boolean flag1=0;
void intFlag1() {
  flag1 = true;
}



/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
void setup() {
  pinMode(PIN_IN_CLOSE, INPUT);
  pinMode(PIN_IN_DOOR_CLOSED, INPUT);
  // set pullups
  digitalWrite(PIN_IN_CLOSE, HIGH);
  digitalWrite(PIN_IN_DOOR_CLOSED, HIGH);

  pinMode(PIN_OUT_OPEN, OUTPUT);
  pinMode(PIN_OUT_CLOSE, OUTPUT);
  pinMode(PIN_OUT_LED, OUTPUT);
  pinMode(PIN_OUT_BUZZER, OUTPUT);

  digitalWrite(PIN_OUT_OPEN, LOW);
  digitalWrite(PIN_OUT_CLOSE, LOW);
  digitalWrite(PIN_OUT_LED, LOW);
  digitalWrite(PIN_OUT_BUZZER, LOW);

  Serial.begin(9600);
  Serial.flush();

  attachInterrupt(0,intFlag1,FALLING);
}


/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
unsigned long lastBlink=0;
boolean blinkOn=true;
unsigned long pinInClosePressedTime=0;

void loop() {
  unsigned long time = millis();

  if(flag1==true) {
    flag1 = false;
    if(pinInClosePressedTime + PIN_IN_CLOSE_HYST_MS < time) {
      commandDoorLockdown();
      time = millis();
      pinInClosePressedTime = time;
    }
  }
  
  
  int serialBytesAvailable = Serial.available();
  while(serialBytesAvailable--) {
    int sread = Serial.read();
    if((sread == '\n') || (sread == '+')) {      
      if(!parseCommand()) {
        ledBlink(10,1);
        delay(200);
        ledBlink(10,1);
      } else {
        ledBlink(10,1);
      }
    }
    else if(sread >= 32 && sread <= 126) {
      strRaw += (char)sread;
    }
    else {
      // read failed
    }
  }
  
  time = millis();
  if( (lastBlink + 1000 < time) || (time < lastBlink)) {
    lastBlink = time;
    blinkOn  = !blinkOn;
  }
  //digitalWrite(PIN_OUT_LED,blinkOn ? HIGH : LOW);
  float ledValue = sin(((double)time)/1500)*128+128;
  analogWrite(PIN_OUT_LED,map(ledValue,0,256,5,250));
}





